//
// Created by 24087 on 2025/5/13.
//

#include "Interface_Motor.h"
const uint8_t CHECK_BYTE = 0x6B;
Queue2 RawAnglePool;
MotorInfo MotorPool[MAX_MOTOR_NUM] = {0};

void RawAnglePool_Init(Queue2 *RawAnglePool)
{
	Queue2Init(RawAnglePool);
}

void RawAnglePool_Destroy(Queue2 *RawAnglePool)
{
	Queue2Destroy(RawAnglePool);
}

void RawAnglePool_Push(Queue2 *RawAnglePool, const RawAngle new_rawAngle)
{
	Queue2Push(RawAnglePool, new_rawAngle);
}

HAL_StatusTypeDef2 RawAngleData_Check(const uint8_t *angleData)
{
	if (angleData[6] == CHECK_BYTE)
	{
		return HAL_OK2;
	}
	return HAL_ERROR2;
}

RawAngle RawAnglePool_Pop(Queue2 *RawAnglePool)
{
	return Queue2Pop(RawAnglePool);
}

void BufferMov_Motor(Queue2 *RawAnglePool, const uint8_t *rawAngleData, const uint8_t ID)
{
	RawAngle new_rawAngle;
	for (int i = 0; i < 7; i++)
	{
		new_rawAngle.rawAngle[i] = rawAngleData[i];
	}
	new_rawAngle.motorID = ID;

	Queue2Push(RawAnglePool, new_rawAngle);
}

bool RawAnglePool_Full(const Queue2 *RawAnglePool)
{
	return Queue2Full(RawAnglePool);
}

bool RawAnglePool_Empty(const Queue2 *RawAnglePool)
{
	return Queue2Empty(RawAnglePool);
}

MotorInfo Parser_Motor(RawAngle rawAngle)
{
	MotorInfo new_info;
	const uint32_t raw = _byteswap_ulong(*(uint32_t *) &rawAngle.rawAngle[2]);
	new_info.current_angle = (rawAngle.rawAngle[1] == 0x00 ? 1.0f : -1.0f) * raw * (2 * 3.1415926535f / 65536.0f) / 30.0f;
	new_info.id = rawAngle.motorID;

	return new_info;
}

void Update_MotorPool(MotorInfo *MotorPool, const MotorInfo new_motorInfo)
{
	MotorPool[new_motorInfo.id - 1] = new_motorInfo;
}

void Motor_Process(Queue2 *RawAnglePool, MotorInfo *MotorPool)
{
	Update_MotorPool(MotorPool, Parser_Motor(RawAnglePool_Pop(RawAnglePool)));
}

float Get_CurrentAngle(const uint8_t id)
{
	return MotorPool[id - 1].current_angle;
}
